
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_rf_backend.c
  * @author     baiyang
  * @date       2023-2-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_rf_backend.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_rf_backend_ctor(sensor_rf_backend_t rf_backend,
                                    sensor_rf_backend_ops_t _ops,
                                    sensor_rf_state_t _state, sensor_rf_params_t _params,
                                    const char *name)
{
    rf_backend->ops = _ops;

    rf_backend->state =_state;
    rf_backend->params = _params;

    rf_backend->_backend_type = sensor_rf_type(rf_backend);

    rf_backend->_mutex = rt_mutex_create(name, RT_IPC_FLAG_FIFO);
}

MAV_DISTANCE_SENSOR sensor_rf_get_mav_distance_sensor_type(sensor_rf_backend_const_t rf_backend)
{
    if (sensor_rf_type(rf_backend) == RANGEFINDER_NONE) {
        return MAV_DISTANCE_SENSOR_UNKNOWN;
    }

    RT_ASSERT(rf_backend->ops->_get_mav_distance_sensor_type != NULL);
    return rf_backend->ops->_get_mav_distance_sensor_type(rf_backend);
}

enum RangeFinderStatus sensor_rf_status(sensor_rf_backend_const_t rf_backend)
{
    if (sensor_rf_type(rf_backend) == RANGEFINDER_NONE) {
        // turned off at runtime?
        return RangeFinderNotConnected;
    }
    return rf_backend->state->status;
}

// true if sensor is returning data
bool sensor_rf_has_data(sensor_rf_backend_const_t rf_backend)
{
    return ((rf_backend->state->status != RangeFinderNotConnected) &&
            (rf_backend->state->status != RangeFinderNoData));
}

// update status based on distance measurement
void sensor_rf_update_status(sensor_rf_backend_t rf_backend)
{
    // check distance
    if (rf_backend->state->distance_m > sensor_rf_max_distance_cm(rf_backend) * 0.01f) {
        sensor_rf_set_status(rf_backend, RangeFinderOutOfRangeHigh);
    } else if (rf_backend->state->distance_m < sensor_rf_min_distance_cm(rf_backend) * 0.01f) {
        sensor_rf_set_status(rf_backend, RangeFinderOutOfRangeLow);
    } else {
        sensor_rf_set_status(rf_backend, RangeFinderGood);
    }
}

// set status and update valid_count
void sensor_rf_set_status(sensor_rf_backend_t rf_backend, enum RangeFinderStatus status)
{
    rf_backend->state->status = status;

    // update valid count
    if (status == RangeFinderGood) {
        if (rf_backend->state->range_valid_count < 10) {
            rf_backend->state->range_valid_count++;
        }
    } else {
        rf_backend->state->range_valid_count = 0;
    }
}

/// 
void sensor_rf_backendDestructor(sensor_rf_backend_t rf_backend)
{
    if (rf_backend->_mutex != NULL) {
        rt_mutex_delete(rf_backend->_mutex);
    }
}
/*------------------------------------test------------------------------------*/


